#include "Haptic.h"
#include "i2c.h"
#include <math.h>


struct hapticSettings Haptic_Setting;


/**
  * @brief  Confirm The Device.
  * @param  reg      ,The register value of the device.
  * @retval Error status
  */
ErrorStatus Confirm_Device(uint8_t reg)
{
    uint8_t revofchip;
    HAL_I2C_Mem_Read(&hi2c1, reg, CHIP_REV_REG, I2C_MEMADD_SIZE_8BIT, &revofchip, 0x01, 0x01);
    
    if(revofchip == 0xBA)
    {
      return SUCCESS;
    }
    else
      return ERROR;
 }

/**
  * @brief  Initialize the device.
  * @param  void
  * @retval Error status
  */
ErrorStatus Init_Motor(void)
{
  Haptic_Setting.motorType = LRA_TYPE;
  Haptic_Setting.absVolt   = 2.5;                   //volts - when acceleration is off, set this only
  Haptic_Setting.nomVolt   = 2.5;                   //volts - not used when acceleration is off, set nominal when on
  Haptic_Setting.currMax   = 165.4;                 //milliamps
  Haptic_Setting.impedance = 13.8;                  //ohms
  Haptic_Setting.lraFreq   = 170;                   //hertz
  
/* some thing here.*/  
  if( !Set_Motor_Type(Haptic_Setting.motorType) &&
      !Set_ABSVolt(Haptic_Setting.absVolt)      &&
      !Set_NomVolt(Haptic_Setting.nomVolt)      &&
      !Set_MaxCurrent(Haptic_Setting.currMax)   &&
      !Set_Impedance(Haptic_Setting.impedance)  &&
      !Set_Frequency(Haptic_Setting.lraFreq)
  )
    return SUCCESS;
  else
    return ERROR;
}


/**
  * @brief  Set the motor type
  * @param  type  ,The register value of the device, LRA_TYPE & ERM_TYPE
  * @retval Error status
  */
ErrorStatus Set_Motor_Type(uint8_t type)
{
  if(type != LRA_TYPE && type != ERM_TYPE)
    return ERROR;
 
  if( !(Write_Register(TOP_CFG1, 0xDF, type, 5)))
    return SUCCESS;
  else
    return ERROR;  
}

/**
  * @brief  Set the absolute maximum rated voltage.
  * @param  absvolt  ,The absolute maximum voltage
  * @retval Error status
  */
ErrorStatus Set_ABSVolt(float absvolt)
{
  if( absvolt < 0 || absvolt >6.0f)
    return ERROR;
  
  absvolt = absvolt / (23.4f * pow(10,-3));
  
  if( !Write_Register(ACTUATOR2 ,0x00, (uint8_t)absvolt, 0))
    return SUCCESS;
  else
    return ERROR;  
}

/**
  * @brief  Set nominal voltage
  * @param  nomvolt  ,The nominal voltage range.
  * @retval Error status
  */
ErrorStatus Set_NomVolt(float nomvolt)
{
  if( nomvolt <0 || nomvolt > 3.3f)
    return ERROR;
  
  nomvolt = nomvolt / (23.4f * pow(10,-3));
  
  if( !Write_Register(ACTUATOR1, 0x00, (uint8_t)nomvolt, 0))
    return SUCCESS;
  else
    return ERROR;
}


/**
  * @brief  Set the maximum rated current
  * @param  maxcurrent  ,The maximum rated current in milliamps.
  * @retval Error status
  */
ErrorStatus Set_MaxCurrent(float maxcurrent)
{
  if( maxcurrent < 0 || maxcurrent > 300.0f)
    return ERROR;
  
  maxcurrent = (maxcurrent - 28.6f) / 7.2f;
  
  if( !Write_Register(ACTUATOR3, 0xE0, (uint8_t)maxcurrent, 0))
    return SUCCESS;
  else 
    return ERROR;
}

/**
  * @brief  Set the impedance, CALIB_V2I_H (0x000F)&CALIB_V2I_L (0x0010)
  * @param  impedance  ,takes the impedance of the motor intended to be paired with the motor driver IC.
                        The value is dependent on the max current set in 0x0E (ACTUATOR3),so be sure to set that first. 
  * @retval Error status
  */
ErrorStatus Set_Impedance(float impedance)
{
  uint8_t  msbimpedance;
  uint8_t  lsbimpedance;
  uint8_t  maxcurrent;
  uint16_t v2ifactor;
  
  if( impedance < 0 || impedance > 50.0f)
    return ERROR;
  
  /*Read the maximum current value.*/
  HAL_I2C_Mem_Read(&hi2c1, DEF_ADDR, ACTUATOR3,I2C_MEMADD_SIZE_8BIT, &maxcurrent,0x01, 0x01);
  maxcurrent = maxcurrent | 0x1F;
  
  v2ifactor = ( impedance * ( maxcurrent + 4)) / 1.6104f;
  msbimpedance = ( v2ifactor - ( v2ifactor & 0x00FF)) / 256;
  lsbimpedance = ( v2ifactor - ( 256 * ( v2ifactor >> 8)));
  
  if( !Write_Register(CALIB_V2I_L, 0x00, lsbimpedance, 0) && 
      !Write_Register(CALIB_V2I_H, 0x00, msbimpedance, 0) )
    return SUCCESS;
  else
    return ERROR;
}


/**
  * @brief  Set motor frequency, FRQ_LRA_PER_H (0x000A)&FRQ_LRA_PER_L (0x000B)
  * @param  frequency  ,Used for specifying the LRA drive frequency.
  * @retval Error status
  */
ErrorStatus Set_Frequency(float frequency)
{
  if( frequency < 0 || frequency >500.0f)
    return ERROR;
  
  uint8_t  msbfrequency;
  uint8_t  lsbfrequency;
  uint16_t lraperiod;
  
  lraperiod = 1 / (frequency * (1333.32f * pow(10,-9)));
  msbfrequency = ( lraperiod - (lraperiod & 0x007F)) / 128;
  lsbfrequency = ( lraperiod - 128 * (lraperiod >> 7));
  
  if( !Write_Register(FRQ_LRA_PER_H, 0x00, msbfrequency, 0) &&
      !Write_Register(FRQ_LRA_PER_L, 0x80, lsbfrequency,0) )
    return SUCCESS;
  else
    return ERROR;
}

/**
  * @brief  Enable resonant frequency tracking; ignored in ERM mode
  * @param  enable  , 1 = enable. 0 = disable
  * @retval Error status
  */
ErrorStatus Enable_FreqTrack(FlagStatus enable)
{
  if( !Write_Register(TOP_CFG1, 0xF7, enable, 3))
    return SUCCESS;
  else 
    return ERROR;
}

/**
  * @brief  DA7280 offers multiple operating modes for use in different applications 
            and to minimize power consumption.
  * @param  enable  , 1 = enable. 0 = disable
  * @retval Error status
  */
ErrorStatus Set_OperatingModes(uint8_t mode)
{
  if( mode > 4)
    return ERROR;
  if( !Write_Register(TOP_CTL1, 0xF8, mode, 0 ))
  {
    HAL_Delay(1);
    return SUCCESS;
  }
  else
  {
    HAL_Delay(1);
    return ERROR;
  }
}


/**
  * @brief  Applies the argument to the register that controls 
            the strength of the vibration. 
            The function first checks if acceleration mode is enabled 
            which limits the maximum value that can be written to the register.
  * @param  value  
  * @retval Error status
  */
ErrorStatus Set_Vibrate(uint8_t value)
{
//  if(value < 0)
//    return ERROR;
  uint8_t acceleration;
  
  HAL_I2C_Mem_Read(&hi2c1, DEF_ADDR, TOP_CFG1, I2C_MEMADD_SIZE_8BIT, &acceleration, 0x01, 0x01);
  acceleration &= 0x04;
  acceleration >>=2;
  
  if( acceleration == ENABLE)
  {
    if( value > 0x7F)
      value = 0x7F;          //Limited to the limit value.
  }
  else
  {
    if( value > 0xFF )
      value = 0xFF;          //Limited to the limit value.
  }

   if( !Write_Register(TOP_CTL2, 0x00, value, 0))
     return SUCCESS;
   else
     return ERROR;
}


/**
  * @brief  Write the register
  * @param  reg      ,The register value of the device.
  * @param  mask     ,Clear the bit to be written.
  * @param  bit      ,Value of a single bit
  * @param  position ,The value to shift left.
  * @retval Error status
  */
ErrorStatus Write_Register(uint8_t reg, uint8_t mask, uint8_t bit, uint8_t position)
{
  uint8_t u8_reg_value;
  HAL_I2C_Mem_Read(&hi2c1, DEF_ADDR, reg, I2C_MEMADD_SIZE_8BIT, &u8_reg_value, 0x01, 0x01);
  
  u8_reg_value &= mask;
  u8_reg_value |= (bit << position);

  if((HAL_I2C_Mem_Write(&hi2c1, DEF_ADDR, reg,I2C_MEMADD_SIZE_8BIT, &u8_reg_value, 0x01, 0x01)) == HAL_OK)
    return SUCCESS;
  else 
    return ERROR;   
}









